import math

def coordinate_to_radians(deg, min = 0.0, sec = 0.0):
    '''Convert a geographical coordinate into radians.'''

    if deg < 0.0:
        sign = -1.0
        deg = -deg
    else:
        sign = 1.0
    print 
    print deg
    print min
    print sec
    print "1:" + str( sec / 60.0 )
    print "2:" + str( (min + sec / 60.0) / 60.0 )
    print "3:" + str( (sign * (deg + (min + sec / 60.0) / 60.0)))
    print "3:" + str( (sign * (deg + (min + sec / 60.0) / 60.0))* math.pi )
    return (sign * (deg + (min + sec / 60.0) / 60.0)) * math.pi / 180.0
    
    
def angular_distance(standpoint_lat, standpoint_long, forepoint_lat, forepoint_long):
    '''Calculate the circular angular distance of two points on a sphere.'''
    print str(standpoint_lat) + " " + str(standpoint_long) + " " + str(forepoint_lat) + " " + str(forepoint_long)

    phi_s = standpoint_lat
    phi_f = forepoint_lat
    lambda_diff = standpoint_long - forepoint_long

    cos = math.cos
    sin = math.sin

    num = (cos(phi_f) * sin(lambda_diff)) ** 2.0 + (cos(phi_s) * sin(phi_f) - sin(phi_s) * cos(phi_f) * cos(lambda_diff)) ** 2.0
    denom = sin(phi_s) * sin(phi_f) + cos(phi_s) * cos(phi_f) * cos(lambda_diff)

    return math.atan2(math.sqrt(num), denom)
    
    

def distance_between_coordinates_km(lat1, long1, lat2, long2):
    '''Calculate distance between two points on Earth in kilometers. lat and long are three-tuples (deg, min, sec).'''

    rad_lat1 = coordinate_to_radians(lat1[0], lat1[1], lat1[2])
    rad_long1 = coordinate_to_radians(long1[0], long1[1], long1[2])
    rad_lat2 = coordinate_to_radians(lat2[0], lat2[1], lat2[2])
    rad_long2 = coordinate_to_radians(long2[0], long2[1], long2[2])

    return angular_distance(rad_lat1, rad_long1, rad_lat2, rad_long2) * 6371.01  # km

def distance_between_dec_coordinates_km(lat1, long1, lat2, long2):
    '''Calculate distance between two points on Earth in kilometers. lat and long are three-tuples (deg, min, sec).'''

    rad_lat1 = lat1 * math.pi / 180.0
    rad_long1 = long1 * math.pi / 180.0
    rad_lat2 = lat2 * math.pi / 180.0
    rad_long2 = long2 * math.pi / 180.0

    return angular_distance(rad_lat1, rad_long1, rad_lat2, rad_long2) * 6371.01  # km

    
if( __name__ == "__main__" ):
    
    """
    
    #Your position is
    #latitude: 48.73244002888791
    #longitude: 8.999691009521484
    
    #latitude: 48.68114617023896
    #longitude: 8.974714279174805
    """
    print "Distance = %f" % distance_between_coordinates_km((36, 7.2, 0), (-86, 40.2, 0), (33, 56.4, 0), (-118, 24.0, 0))
    print "Distance = %f" % distance_between_coordinates_km((48, 43, 56.78), (8, 59, 58.89), (48, 40, 52.13), (8, 58, 28.97))
